dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * shot_local_estimator.h
   3  *
   4  *  Created on: Mar 24, 2012
   5  *      Author: aitor
   6  */
   7
   8 #pragma once
   9
  10 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
  11 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
  12 #include <pcl/features/shot_omp.h>
  13 #include <pcl/io/pcd_io.h>
  14
  15 namespace pcl {
  16 namespace rec_3d_framework {
  17
  18 template <typename PointInT, typename FeatureT>
  19 class SHOTLocalEstimationOMP : public LocalEstimator<PointInT, FeatureT> {
  20
  21   using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  22   using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  23   using KeypointCloud = pcl::PointCloud<pcl::PointXYZ>;
  24
  25   using LocalEstimator<PointInT, FeatureT>::support_radius_;
  26   using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  27   using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  28   using LocalEstimator<PointInT, FeatureT>::neighborhood_indices_;
  29   using LocalEstimator<PointInT, FeatureT>::neighborhood_dist_;
  30   using LocalEstimator<PointInT, FeatureT>::adaptative_MLS_;
  31
  32 public:
  33   bool
  34   estimate(PointInTPtr& in,
  35            PointInTPtr& processed,
  36            PointInTPtr& keypoints,
  37            FeatureTPtr& signatures) override
  38   {
  39     if (!normal_estimator_) {
  40       PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs normals... please "
  41                 "provide a normal estimator\n");
  42       return false;
  43     }
  44
  45     if (keypoint_extractor_.empty()) {
  46       PCL_ERROR("SHOTLocalEstimationOMP :: This feature needs a keypoint extractor... "
  47                 "please provide one\n");
  48       return false;
  49     }
  50
  51     pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  52     pcl::MovingLeastSquares<PointInT, PointInT> mls;
  53     if (adaptative_MLS_) {
  54       typename search::KdTree<PointInT>::Ptr tree;
  55       Eigen::Vector4f centroid_cluster;
  56       pcl::compute3DCentroid(*in, centroid_cluster);
  57       float dist_to_sensor = centroid_cluster.norm();
  58       float sigma = dist_to_sensor * 0.01f;
  59       mls.setSearchMethod(tree);
  60       mls.setSearchRadius(sigma);
  61       mls.setUpsamplingMethod(mls.SAMPLE_LOCAL_PLANE);
  62       mls.setUpsamplingRadius(0.002);
  63       mls.setUpsamplingStepSize(0.001);
  64     }
  65
  66     normals.reset(new pcl::PointCloud<pcl::Normal>);
  67     {
  68       pcl::ScopeTime t("Compute normals");
  69       normal_estimator_->estimate(in, processed, normals);
  70     }
  71
  72     if (adaptative_MLS_) {
  73       mls.setInputCloud(processed);
  74
  75       PointInTPtr filtered(new pcl::PointCloud<PointInT>);
  76       mls.process(*filtered);
  77
  78       processed.reset(new pcl::PointCloud<PointInT>);
  79       normals.reset(new pcl::PointCloud<pcl::Normal>);
  80       {
  81         pcl::ScopeTime t("Compute normals after MLS");
  82         filtered->is_dense = false;
  83         normal_estimator_->estimate(filtered, processed, normals);
  84       }
  85     }
  86
  87     this->computeKeypoints(processed, keypoints, normals);
  88     std::cout << " " << normals->size() << " " << processed->size() << std::endl;
  89
  90     if (keypoints->points.empty()) {
  91       PCL_WARN("SHOTLocalEstimationOMP :: No keypoints were found\n");
  92       return false;
  93     }
  94
  95     // compute signatures
  96     using SHOTEstimator = pcl::SHOTEstimationOMP<PointInT, pcl::Normal, pcl::SHOT352>;
  97     typename pcl::search::KdTree<PointInT>::Ptr tree(new pcl::search::KdTree<PointInT>);
  98     tree->setInputCloud(processed);
  99
 100     pcl::PointCloud<pcl::SHOT352>::Ptr shots(new pcl::PointCloud<pcl::SHOT352>);
 101     SHOTEstimator shot_estimate;
 102     shot_estimate.setNumberOfThreads(8);
 103     shot_estimate.setSearchMethod(tree);
 104     shot_estimate.setInputCloud(keypoints);
 105     shot_estimate.setSearchSurface(processed);
 106     shot_estimate.setInputNormals(normals);
 107     shot_estimate.setRadiusSearch(support_radius_);
 108
 109     {
 110       pcl::ScopeTime t("Compute SHOT");
 111       shot_estimate.compute(*shots);
 112     }
 113
 114     signatures->resize(shots->size());
 115     signatures->width = shots->size();
 116     signatures->height = 1;
 117
 118     int size_feat = sizeof((*signatures)[0].histogram) / sizeof(float);
 119
 120     int good = 0;
 121     for (const auto& point : shots->points) {
 122
 123       int NaNs = 0;
 124       for (int i = 0; i < size_feat; i++) {
 125         if (!std::isfinite(point.descriptor[i]))
 126           NaNs++;
 127       }
 128
 129       if (NaNs == 0) {
 130         for (int i = 0; i < size_feat; i++) {
 131           (*signatures)[good].histogram[i] = point.descriptor[i];
 132         }
 133
 134         good++;
 135       }
 136     }
 137     signatures->resize(good);
 138     signatures->width = good;
 139
 140     return true;
 141   }
 142 };
 143
 144 } // namespace rec_3d_framework
 145 } // namespace pcl